import numpy as np
import matplotlib.pyplot as plt
import os
import random


# def set_seed(seed=308):
#     random.seed(seed)
#     os.environ["PYTHONHASHSEED"] = str(seed)
#     np.random.seed(seed)
#
#
# set_seed(308)  # 固定随机种子，方便结果复现


plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题
plt.rc('font', size=10)
plt.rc('font', family='SimHei')

# 通用常数
interval = 1  # 投弹时间间隔
v_down = 3  # 云团下降速度
r_effective = 10  # 有效遮蔽半径
t_effective = 20  # 有效遮蔽时间
v_missile = 300  # 导弹速度
r_target = 7  # 目标半径
H_target = 10  # 目标高度
target = np.array([0, 200, 0])  # 目标底部圆心坐标
M1 = np.array([20000, 0, 2000])
M2 = np.array([19000, 600, 2100])
M3 = np.array([18000, -600, 1900])
FY1 = np.array([17800, 0, 1800])
FY2 = np.array([12000, 1400, 1400])
FY3 = np.array([6000, -3000, 700])
FY4 = np.array([11000, 2000, 1800])
FY5 = np.array([13000, -2000, 1300])
v_range = np.array([70, 140])  # 无人机速度范围
g = 9.8  # 重力加速度
# target_point = np.array([
#     [(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, 0],
#     [-(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, 0],
#     [(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, 0],
#     [-(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, 0],
#     [(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target],
#     [-(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target],
#     [(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target],
#     [-(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target],
#     [(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target / 2],
#     [-(1 / 2) ** 0.5 * r_target, (1 / 2) ** 0.5 * r_target + 200, H_target / 2],
#     [(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target / 2],
#     [-(1 / 2) ** 0.5 * r_target, -(1 / 2) ** 0.5 * r_target + 200, H_target / 2],
# ])
target_point = np.array([
    [0, 200, 0]
])
t_total = np.linalg.norm(M1) / 300  # 从0开始到导弹击中目标的总用时
precision = 0.01  # 时间精度

# 传入M1的初始坐标（也即可确定其飞行方向）和经过的时间，返回其在给定时间下的坐标
def get_M1_coordinate(coordinate_0, t, v=None):
    if not v:
        v = -coordinate_0 / np.linalg.norm(coordinate_0) * v_missile
    else:
        v = v / np.linalg.norm(v) * v_missile
    return coordinate_0 + v * t


# 传入FY1的坐标和经过的时间，返回它投射的第i枚烟雾弹中心的坐标
def get_FY1_coordinate(coordinate_0, t, i, v_1, t_1, t_2, theta):
    v = np.array([np.cos(theta), np.sin(theta), 0])

    if t < t_1 + i * t_2 or t > t_1 + i * t_2 + t_effective:
        return [None, None, None]
    else:
        v[2] = 0
        v = v / np.linalg.norm(v) * v_1
        delta_t = t - t_1 - t_2
        coordinate = coordinate_0 + v * (t_1 + t_2)
        coordinate[2] -= 1 / 2 * g * (t_2 ** 2)
        coordinate[2] -= delta_t * v_down
        return coordinate


def collision_detection(coordinate_missile, coordinate_bomb):
    if coordinate_bomb[0] == None:
        return False
    global target_point, r_effective
    count_collision = 0
    for target in target_point:
        l_1 = coordinate_bomb - target
        l_2 = coordinate_missile - target
        molecule = np.cross(l_1, l_2)
        denominator = np.linalg.norm(l_2)
        d = np.linalg.norm(molecule) / denominator
        if d < r_effective:
            missile_to_target = -coordinate_missile  # 由导弹指向目标
            missile_to_bomb = coordinate_bomb - coordinate_missile  # 由导弹指向烟幕弹
            cos = np.dot(missile_to_target, missile_to_bomb)
            if cos > 0 or np.linalg.norm(missile_to_bomb) < r_effective:  # 余弦值大于0，是锐角，或者导弹和烟幕弹之间的距离小于有效半径
                count_collision += 1
    return count_collision


# 飞行方向、飞行速度、烟幕干扰弹投放点、烟幕干扰弹起爆点
def fun(theta, v_1, t_1, t_2):
    t_list = np.arange(t_1 + t_2, t_1 + t_2 + t_effective, precision)  # 时间序列，终点是0
    count = 0
    for t in t_list:
        if collision_detection(get_M1_coordinate(M1, t), get_FY1_coordinate(FY3, t, 1, v_1, t_1, t_2, theta)) == len(target_point):
            count += 1
    return count

print(fun(1.4277771491526756, 79.47410529756488, 35.41109849256417, 3.1421727469991856))
# # 在SA函数外部初始化记录容器
# history = {
#     'temperature': [],
#     'best_P': [],
#     'current_P': [],
#     'k': [[], [], [], [], []],
# }
#
#
# def plot_SA_history():
#     plt.figure(figsize=(15, 8))
#
#     # 子图1：损失函数变化
#     plt.subplot(2, 3, 1)
#     plt.plot(history['best_P'], 'r-', label='Best P')
#     plt.plot(history['current_P'], 'b--', alpha=0.5, label='Current P')
#     plt.xlabel('Iteration')
#     plt.ylabel('P')
#     plt.title('P变化曲线')
#     plt.legend()
#     plt.grid(True, which="both", ls="--")
#
#     # 子图2：温度衰减曲线
#     plt.subplot(2, 3, 2)
#     plt.plot(history['temperature'], 'g-')
#     plt.xlabel('Iteration')
#     plt.ylabel('Temperature')
#     plt.title('温度下降曲线')
#     plt.grid(True, ls="--")
#
#     # 子图3：k参数演化
#     plt.subplot(2, 3, 3)
#     plt.plot(history['k'][0], label="无人机运动方向")
#     plt.xlabel('Iteration')
#     plt.ylabel('value')
#     plt.title('目标点参数变化曲线')
#     plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left')
#     plt.grid(True, ls="--")
#
#     # 子图3：k参数演化
#     plt.subplot(2, 3, 4)
#     plt.plot(history['k'][1], label="无人机速度大小")
#     plt.xlabel('Iteration')
#     plt.ylabel('value')
#     plt.title('飞行速度变化曲线')
#     plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left')
#     plt.grid(True, ls="--")
#
#     # 子图3：k参数演化
#     plt.subplot(2, 3, 5)
#     plt.plot(history['k'][2], label="投弹时间")
#     plt.plot(history['k'][3], label="爆炸时间")
#     plt.xlabel('Iteration')
#     plt.ylabel('value')
#     plt.title('时间参数变化曲线')
#     plt.legend(bbox_to_anchor=(1.05, 1), loc='upper left')
#     plt.grid(True, ls="--")
#
#     plt.tight_layout()
#     plt.show()
#
#
# def SA(iter, t0, tf, alpha):
#     t = t0
#     theta_c = 1.4274729345586177
#     v_1_c = 80.46564965540898
#     t_1_c = 34.959595046157155
#     t_2_c = 3.194123048935585
#     Pc = fun(theta_c, v_1_c, t_1_c, t_2_c)
#     theta_b = theta_c
#     v_1_b = v_1_c
#     t_1_b = t_1_c
#     t_2_b = t_2_c
#     Pb = Pc
#     for i in range(iter):
#         theta_n = theta_c + np.random.normal(0, 0.05)
#         v_1_n = v_1_c + np.random.normal(0, 0.05)
#         t_1_n = t_1_c + np.random.normal(0, 0.05)
#         t_2_n = t_2_c + np.random.normal(0, 0.05)
#
#         theta_n = np.clip(theta_n, 0, 2 * np.pi)
#         v_1_n = np.clip(v_1_n, v_range[0], v_range[1])
#         t_1_n = np.clip(t_1_n, 0, t_total)
#         t_2_n = np.clip(t_2_n, 0, t_total)
#         Pn = fun(theta_n, v_1_n, t_1_n, t_2_n)
#         if Pn >= Pc or np.random.rand() < np.exp((Pn-Pc)/t):
#             theta_c = theta_n
#             v_1_c = v_1_n
#             t_1_c = t_1_n
#             t_2_c = t_2_n
#             Pc = Pn
#             if Pc > Pb:
#                 theta_b = theta_c
#                 v_1_b = v_1_c
#                 t_1_b = t_1_c
#                 t_2_b = t_2_c
#                 Pb = Pc
#         t = t*alpha
#         if t < tf:
#             break
#         print(f"第{i}轮,Pb:{Pb},Pc:{Pc},kb:{float(theta_b), float(v_1_b), float(t_1_b), float(t_2_b)},kc:{(float(theta_c), float(v_1_c), float(t_1_c), float(t_2_c))}")
#         history['temperature'].append(t)
#         history['best_P'].append(Pb)
#         history['current_P'].append(Pc)
#         history['k'][0].append(theta_b)
#         history['k'][1].append(v_1_b)
#         history['k'][2].append(t_1_b)
#         history['k'][3].append(t_2_b)
#     print(f"无人机FY3从初始位置向着{theta_b}飞, 速度为{float(v_1_b)}m/s,开始投弹时刻为{float(t_1_b)}s, 爆炸间隔为{float(t_2_b)}s, 最终有效遮挡时间为{float(Pb * precision)}")
#     return (theta_b, v_1_b, t_1_b, t_2_b), Pb
#
#
# re = SA(300, 300, 0.001, 0.95)
# print("_________________________________________")
# print(re)
# # plot_SA_history()
